﻿using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Text;
using System.Windows.Forms;
using System.IO;
using System.IO.Ports;
using System.Threading;

using System.Drawing.Imaging;

using AForge;
using AForge.Robotics.Lego;
using AForge.Video;
using AForge.Video.DirectShow;
using AForge.Imaging;
using AForge.Imaging.Filters;

using CokAracliKontrol.PathFinding;
using CokAracliKontrol.VehicleControl;

namespace CokAracliKontrol
{
    public partial class MainForm : Form
    {
        ColorSelection ColorSelectionForm;

        Pathfinder _pathfinder = new Pathfinder();
        ILegoDriver legoDriver = null;
        
        private bool onAir = false;
        private bool startMove = false;
        
        // maximum motors' power for 5 sectors 
        private float[] maxPowers = new float[] { 0.1f, 0.4f, 1.0f, 1.0f, 1.0f };

        // background thread and objects used for synchronization
        private Thread thread = null;
        private AutoResetEvent needToDriveEvent = null;
        private bool needToExit = false;

        // motors' power to set
        private float leftMotorPower = 0;
        private float rightMotorPower = 0;

        ColorFiltering colorFilter = new ColorFiltering();
        GrayscaleBT709 grayFilter = new GrayscaleBT709();
        BlobCounter blobCounter = new BlobCounter();

        Bitmap capturedImage = null;

        Dictionary<int, List<Point>> paths = new Dictionary<int,List<Point>>();

        List<int> sortedVehicleIDs = new List<int>();
        List<int> sortedVehiclePriorities = new List<int>();

        Dictionary<int, int> vehicleStepCounters = new Dictionary<int, int>();

        //VideoCaptureDevice videoSource;

        private void Recalculate(int vehicleID)
        {
            _pathfinder.ClearLogic();
            _pathfinder.Pathfind(vehicleID);
            _pathfinder.HighlightPath(vehicleID);
            _pathfinder.DrawBoard(boardControl1, vehicleID);
        }

        private void CalculateVehiclePath(int vehicleID)
        {
            //Recalculate(vehicleID);
            List<Point> path = _pathfinder.HighlightPath(vehicleID);

            //string pr = "";

            paths[vehicleID] = new List<Point>();

            foreach (Point p in path)
            {
                //pr = pr + p.ToString() + "\n";
                lbPathList.Items.Add("Vehicle #" + vehicleID.ToString() + ": " + p.ToString());
                paths[vehicleID].Add(p);
            }

            //MessageBox.Show(pr);
            //pr = "";

            for (int i = 1; i < path.Count; i++)
            {
                Point movement;
                movement = Pathfinder.ExtractMovement(path[i - 1], path[i]);
                lbMotionList.Items.Add("Vehicle #" + vehicleID.ToString() + ": " + movement.ToString());
            }

            vehicleStepCounters[vehicleID] = 0;

            //MessageBox.Show(pr);
        }

        private void ParseColorRanges(string s, IntRange redRange, IntRange greenRange, IntRange blueRange)
        {
            string sred, sblue, sgreen;
            string[] parts;
            parts = s.Split('|');

            sred = parts[0];
            sgreen = parts[1];
            sblue = parts[2];

            parts = sred.Split('-');
            redRange.Min = Convert.ToInt32(parts[0]);
            redRange.Max = Convert.ToInt32(parts[1]);

            parts = sgreen.Split('-');
            greenRange.Min = Convert.ToInt32(parts[0]);
            greenRange.Max = Convert.ToInt32(parts[1]);

            parts = sblue.Split('-');
            blueRange.Min = Convert.ToInt32(parts[0]);
            blueRange.Max = Convert.ToInt32(parts[1]);
        }

        private void ColorSelectionForm_OnFilterUpdate(object sender, EventArgs eventArgs)
        {
            int selectedRow = dataGridViewVehicle.CurrentRow.Index;

            colorFilter.Red = ColorSelectionForm.RedRange;
            colorFilter.Green = ColorSelectionForm.GreenRange;
            colorFilter.Blue = ColorSelectionForm.BlueRange;

            //string pat = colorFilter.Red.Length.ToString() + "," + colorFilter.Green.Length.ToString() + "," + colorFilter.Blue.Length.ToString();

            string pat = "";
            pat += colorFilter.Red.Min.ToString() + "-" + colorFilter.Red.Max.ToString();
            pat += "|" + colorFilter.Green.Min.ToString() + "-" + colorFilter.Green.Max.ToString();
            pat += "|" + colorFilter.Blue.Min.ToString() + "-" + colorFilter.Blue.Max.ToString();

            dataGridViewVehicle["dgVehiclePattern", selectedRow].Value = pat;
        }

        private void ModifyInitialPath(int vehicleID, int xx, int yy)
        {
            for (int i = 0; i < _pathfinder.Squares.GetLength(0); i++)
            {
                for (int j = 0; j < _pathfinder.Squares.GetLength(1); j++)
                {
                    CompleteSquare mysq;
                    mysq = _pathfinder.Squares[i, j];
                    if (mysq.ContentCode == SquareContent.BaseStart + vehicleID)
                    {
                        mysq.ContentCode = SquareContent.FreePath;
                    }
                }
            }
            _pathfinder.Squares[xx, yy].ContentCode = SquareContent.BaseStart + vehicleID;
        }

        private void GetVehiclePosition(int vehicleID, ref int x, ref int y)
        {
            IntRange red, green, blue;

            red = new IntRange(0, 0);
            green = new IntRange(0, 0);
            blue = new IntRange(0, 0);

            dataGridViewVehicle.CurrentCell = dataGridViewVehicle.Rows[vehicleID - 1].Cells["dgVehiclePattern"];

            string colorData;
            colorData = dataGridViewVehicle.CurrentCell.Value.ToString();

            ParseColorRanges(colorData, red, green, blue);

            ColorFiltering vehicleFilter;
            vehicleFilter = new ColorFiltering(red, green, blue);

            Bitmap objectImage = vehicleFilter.Apply(capturedImage);
            
            //objectImage.MakeTransparent(Color.Black);

            // lock image for further processing
            BitmapData objectData = objectImage.LockBits(
                new Rectangle(0, 0, capturedImage.Width, capturedImage.Height),
                ImageLockMode.ReadOnly, capturedImage.PixelFormat);

            // grayscaling
            UnmanagedImage grayImage = grayFilter.Apply(new UnmanagedImage(objectData));

            // unlock image
            objectImage.UnlockBits(objectData);

            // locate blobs 
            blobCounter.ProcessImage(grayImage);
            Rectangle[] rects = blobCounter.GetObjectsRectangles();
            Rectangle vehicleArea;

            if (rects.Length > 0)
            {
                vehicleArea = rects[0];

                // get object's center coordinates relative to image center
                lock (this)
                {
                    int centerX, centerY;
                    centerX = (vehicleArea.Left + vehicleArea.Right) / 2;
                    centerY = (vehicleArea.Top + vehicleArea.Bottom) / 2;


                    x = centerX / (grayImage.Width / Pathfinder.gridSizeX);
                    y = centerY / (grayImage.Height / Pathfinder.gridSizeY);
                }
            }
            else
            {
                x = -1;
                y = -1;

                //objectImage.Dispose();
                //grayImage.Dispose();
            }

            //ColorSelectionForm.UpdateObjectPicture(capturedImage);
        }

        public MainForm()
        {
            InitializeComponent();

            boardControl1.MouseClick += new MouseEventHandler(boardControl1_MouseClick);
            boardControl1.MouseMoveSpecial +=new EventHandler(boardControl1_MouseMoveSpecial);
            statusStrip1.Text = "";
            toolStripStatusLabel1.Text = "";
            dgConnectionPort.Items.Clear();
            dataGridViewVehicle["dgVehicleID", 0].Value = "1";
            dataGridViewVehicle.Columns["dgVehicleID"].ReadOnly = true;
            
            try
            {
                dgConnectionPort.Items.AddRange(SerialPort.GetPortNames());
            }
            catch (Exception ex)
            {
                MessageBox.Show(ex.Message);
            }

            colorFilter.Red = new IntRange(216, 255);
            colorFilter.Green = new IntRange(216, 255);
            colorFilter.Blue = new IntRange(0, 140);

            blobCounter.MinWidth = 15;
            blobCounter.MinHeight = 15;
            blobCounter.FilterBlobs = true;
            blobCounter.ObjectsOrder = ObjectsOrder.Size;
        }

        private void MainForm_FormClosed(object sender, FormClosedEventArgs e)
        {
            boardControl1.SignalToStop();
            boardControl1.WaitForStop();
            if (ColorSelectionForm != null)
                ColorSelectionForm.UpdateObjectPicture(null);
        }

        private void boardControl1_MouseMoveSpecial(object sender, EventArgs e)
        {
            /*
             * 
             * Called when the mouse moves to a different square.
             * 
             * */
            
            Point point = boardControl1.Pos;
            if (point.X == -1 || point.Y == -1)
            {
                return;
            }
            int count = _pathfinder.Squares[point.X, point.Y].DistanceSteps;
            if (count == Pathfinder.bigNumber)
            {
                toolStripStatusLabel1.Text = "No possible path.";
            }
            else if (count == 1)
            {
                toolStripStatusLabel1.Text = "1 step";
            }
            else if (count == 0)
            {
                toolStripStatusLabel1.Text = "Current";
            }
            else
            {
                toolStripStatusLabel1.Text = count.ToString() + " steps.";
            }

            toolStripStatusLabel1.Text += " X:" + point.X.ToString() + " Y:" + point.Y.ToString();
        }

        private void boardControl1_MouseClick(object sender, MouseEventArgs e)
        {
            Point point = boardControl1.Pos;
            int selectedRow = dataGridViewVehicle.CurrentRow.Index;
            int vehicleID = Convert.ToInt32(dataGridViewVehicle.Rows[selectedRow].Cells["dgVehicleID"].Value);

            if (point.X == -1 || point.Y == -1)
            {
                return;
            }

            if (e.Button == MouseButtons.Left)
            {
                for (int row = 0; row < Pathfinder.gridSizeY; row++)
                {
                    for (int col = 0; col < Pathfinder.gridSizeX; col++)
                    {
                        if (_pathfinder.Squares[col, row].ContentCode == SquareContent.FinishLocation(vehicleID))
                        {
                            _pathfinder.Squares[col, row].ContentCode = SquareContent.FreePath;
                            break;
                        }
                    }
                }

                _pathfinder.Squares[point.X, point.Y].ContentCode = SquareContent.FinishLocation(vehicleID);
                dataGridViewVehicle.Rows[selectedRow].Cells["dgTargetCoordinate"].Value = point.X.ToString() + "," + point.Y.ToString();

                for (int i = 0; i < dataGridViewVehicle.RowCount - 1; i++)
                {
                    Recalculate(Convert.ToInt32(dataGridViewVehicle["dgVehicleID", i].Value));
                }
                //Recalculate(vehicleID);
            }

            if (e.Button == MouseButtons.Right)
            {
                if (_pathfinder.Squares[point.X, point.Y].ContentCode == SquareContent.FreePath)
                {
                    _pathfinder.Squares[point.X, point.Y].ContentCode = SquareContent.Obstacle;
                    Recalculate(vehicleID);
                }
                else if (_pathfinder.Squares[point.X, point.Y].ContentCode == SquareContent.Obstacle)
                {
                    _pathfinder.Squares[point.X, point.Y].ContentCode = SquareContent.FreePath;
                    Recalculate(vehicleID);
                }
            }
        }

        private void btnSelectCamera_Click(object sender, EventArgs e)
        {
            CameraSelection form = new CameraSelection();
            if (form.ShowDialog(this) == DialogResult.OK)
            {
                if (onAir)
                {
                    boardControl1.SignalToStop();
                    boardControl1.WaitForStop();

                    if (ColorSelectionForm != null)
                        ColorSelectionForm.UpdateObjectPicture(null);

                    timerMoveAgents.Enabled = false;
                    onAir = false;
                    btnSelectCamera.Text = "Start Capture";
                }
                else
                {
 
                    VideoCaptureDevice videoSource = new VideoCaptureDevice(form.localCamera);
                    videoSource.DesiredFrameSize = new Size(640, 480);
                    //videoSource.DesiredFrameRate = 10;

                    boardControl1.VideoSource = videoSource;
                    boardControl1.Start();

                    onAir = true;
                    btnSelectCamera.Text = "Stop Capture";
                }
            }
            
        }

        private void boardControl1_NewFrame(object sender, ref Bitmap image)
        {
            //if (capturedImage != null) capturedImage.Dispose();
            //capturedImage = image;

            capturedImage = (Bitmap)image.Clone();

            if (ColorSelectionForm != null)
            {
                Bitmap img = colorFilter.Apply(image);
                ColorSelectionForm.UpdateObjectPicture(img);
            }
        }

        private void btnCalculate_Click(object sender, EventArgs e)
        {
            int priority;

            for (int i = 0; i < dataGridViewVehicle.RowCount - 1; i++)
            {
                int vehicleID = Convert.ToInt32(dataGridViewVehicle.Rows[i].Cells["dgVehicleID"].Value);

                string initialCoordinate = dataGridViewVehicle.Rows[i].Cells["dgInitialCoordinate"].Value.ToString();
                string[] iCor = initialCoordinate.Split(',');

                string targetCoordinate = dataGridViewVehicle.Rows[i].Cells["dgTargetCoordinate"].Value.ToString();
                string[] tCor = targetCoordinate.Split(',');

                Point iPoint = new Point();
                iPoint.X = Convert.ToInt32(iCor[0]);
                iPoint.Y = Convert.ToInt32(iCor[1]);

                Point tPoint = new Point();
                tPoint.X = Convert.ToInt32(tCor[0]);
                tPoint.Y = Convert.ToInt32(tCor[1]);

                if (iPoint.X == -1 || iPoint.Y == -1)
                {
                    return;
                }

                if (tPoint.X == -1 || tPoint.Y == -1)
                {
                    return;
                }

                for (int row = 0; row < Pathfinder.gridSizeY; row++)
                {
                    for (int col = 0; col < Pathfinder.gridSizeX; col++)
                    {
                        if (_pathfinder.Squares[col, row].ContentCode == SquareContent.StartLocation(vehicleID))
                        {
                            _pathfinder.Squares[col, row].ContentCode = SquareContent.FreePath;
                            break;
                        }

                        if (_pathfinder.Squares[col, row].ContentCode == SquareContent.FinishLocation(vehicleID))
                        {
                            _pathfinder.Squares[col, row].ContentCode = SquareContent.FreePath;
                            break;
                        }
                    }
                }

                _pathfinder.Squares[iPoint.X, iPoint.Y].ContentCode = SquareContent.StartLocation(vehicleID);
                _pathfinder.Squares[tPoint.X, tPoint.Y].ContentCode = SquareContent.FinishLocation(vehicleID);
                Recalculate(vehicleID);

                priority = _pathfinder.HighlightPath(vehicleID).Count;
                dataGridViewVehicle.Rows[i].Cells["dgPriority"].Value = priority.ToString();

                CalculateVehiclePath(vehicleID);
            }

            // priority assingment
        }

        private void dataGridViewVehicle_SelectionChanged(object sender, EventArgs e)
        {
            int selectedRow = dataGridViewVehicle.CurrentRow.Index;

            if (dataGridViewVehicle.Rows[selectedRow].Cells["dgInitialCoordinate"].Value != null &&
                dataGridViewVehicle.Rows[selectedRow].Cells["dgTargetCoordinate"].Value != null)
            {
                int vehicleID = selectedRow + 1;

                string initialCoordinate = dataGridViewVehicle.Rows[selectedRow].Cells["dgInitialCoordinate"].Value.ToString();
                string[] iCor = initialCoordinate.Split(',');

                string targetCoordinate = dataGridViewVehicle.Rows[selectedRow].Cells["dgTargetCoordinate"].Value.ToString();
                string[] tCor = targetCoordinate.Split(',');

                Point iPoint = new Point();
                iPoint.X = Convert.ToInt32(iCor[0]);
                iPoint.Y = Convert.ToInt32(iCor[1]);

                Point tPoint = new Point();
                tPoint.X = Convert.ToInt32(tCor[0]);
                tPoint.Y = Convert.ToInt32(tCor[1]);

                if (iPoint.X == -1 || iPoint.Y == -1)
                {
                    return;
                }

                if (tPoint.X == -1 || tPoint.Y == -1)
                {
                    return;
                }

                for (int row = 0; row < Pathfinder.gridSizeY; row++)
                {
                    for (int col = 0; col < Pathfinder.gridSizeX; col++)
                    {
                        if (_pathfinder.Squares[col, row].ContentCode == SquareContent.StartLocation(vehicleID))
                        {
                            _pathfinder.Squares[col, row].ContentCode = SquareContent.FreePath;
                            break;
                        }

                        if (_pathfinder.Squares[col, row].ContentCode == SquareContent.FinishLocation(vehicleID))
                        {
                            _pathfinder.Squares[col, row].ContentCode = SquareContent.FreePath;
                            break;
                        }
                    }
                }

                _pathfinder.Squares[iPoint.X, iPoint.Y].ContentCode = SquareContent.StartLocation(vehicleID);
                _pathfinder.Squares[tPoint.X, tPoint.Y].ContentCode = SquareContent.FinishLocation(vehicleID);

                Recalculate(vehicleID);
            }
        }

        private void dataGridViewVehicle_RowsAdded(object sender, DataGridViewRowsAddedEventArgs e)
        {
            if (dataGridViewVehicle[0, e.RowIndex].Value == null)
            {
                dataGridViewVehicle[0, e.RowIndex].Value = dataGridViewVehicle.RowCount;
            }
        }

        private void dataGridViewVehicle_CellContentDoubleClick(object sender, DataGridViewCellEventArgs e)
        {
            int selectedRow = dataGridViewVehicle.CurrentRow.Index;
            int vehicleID = selectedRow + 1;

            if (ColorSelectionForm == null)
            {
                ColorSelectionForm = new ColorSelection();
                ColorSelectionForm.OnFilterUpdate += new EventHandler(ColorSelectionForm_OnFilterUpdate);

                ColorSelectionForm.RedRange = colorFilter.Red;
                ColorSelectionForm.GreenRange = colorFilter.Green;
                ColorSelectionForm.BlueRange = colorFilter.Blue;

                string pat = "";
                pat += colorFilter.Red.Min.ToString() + "-" + colorFilter.Red.Max.ToString();
                pat += "|" + colorFilter.Green.Min.ToString() + "-" + colorFilter.Green.Max.ToString();
                pat += "|" + colorFilter.Blue.Min.ToString() + "-" + colorFilter.Blue.Max.ToString();

                dataGridViewVehicle["dgVehiclePattern", selectedRow].Value = pat;

                //MessageBox.Show(pat);
            }

            ColorSelectionForm.Show();
        }

        private void btnConnect_Click(object sender, EventArgs e)
        {
            List<LegoDriver> vehicleDrivers = new List<LegoDriver>();
            string deviceName;
            int SignalStrength;
            int BatteryLevel;

            for (int i = 0; i < dataGridViewVehicle.RowCount - 1; i++)
            {
                vehicleDrivers.Add(new LegoDriver(dataGridViewVehicle["dgConnectionPort",i].Value.ToString()));        
                if (!vehicleDrivers[i].Connect())
	            {
                    string vehicleName = dataGridViewVehicle["dgVehicleName",i].Value.ToString();
                    toolStripStatusLabel1.Text += "Unable to connect " + vehicleName;
                    vehicleDrivers[i] = null;
	            }
                else
                {
                    toolStripStatusLabel1.Text = "All vehicles are succesfully connected.";
                    
                    vehicleDrivers[i].GetDeviceInformation(out deviceName,out SignalStrength,out BatteryLevel);
                    dataGridViewVehicle["dgVehicleName", i].Value = deviceName;
                    //dataGridViewVehicle["dgSignalStrength", i].Value = SignalStrength;
                    dataGridViewVehicle["dgBatteryLevel", i].Value = BatteryLevel;
                    
                    // create event used to signal thread about updates to power state
                    needToDriveEvent = new AutoResetEvent(false);
                    needToExit = false;

                    // create background thread wich drives Lego
                    thread = new Thread(new ThreadStart(WorkerThread));
                    thread.Start();
                }
            }

            
        }

        // Worker thread which is used to set motors' power
        private void WorkerThread()
        {
            float newLeftMotorPower = 0;
            float newRightMotorPower = 0;

            while (true)
            {
                // wait for events
                needToDriveEvent.WaitOne();

                // does thread should exit?
                if (needToExit)
                    break;

                lock (this)
                {
                    newLeftMotorPower = leftMotorPower;
                    newRightMotorPower = rightMotorPower;
                }

                legoDriver.SetMotorsPower(newLeftMotorPower, newRightMotorPower);
            }
        }

        #region My Color Extraction
        //public void colorExtraction(Bitmap image,int red,int green, int blue,double t)
        //{
        //    for (int w = 0; w < image.Width-1; w++)
        //    {
        //        for (int h = 0; h < image.Height-1; h++)
        //        {
        //            Color px = image.GetPixel(w, h);

        //            if (Math.Sqrt(Math.Pow(px.R - red,2) + Math.Pow(px.G - green,2) + Math.Pow(px.B - blue,2)) <= t)
        //            {
        //                image.SetPixel(w, h, Color.Yellow);
        //            }
        //            else
        //            {
        //                image.SetPixel(w, h, Color.Black);
        //            }
        //        } 
        //    }
        //    ColorSelectionForm.UpdateObjectPicture(image);
        //}
        #endregion

        private void timerMoveAgents_Tick(object sender, EventArgs e)
        {
            //foreach (int vehicleID in sortedVehicleIDs)
            //{
            //    Point myStep;

            //    if (vehicleStepCounters[vehicleID] < paths[vehicleID].Count)
            //    {
            //        myStep = paths[vehicleID][vehicleStepCounters[vehicleID]];
            //        vehicleStepCounters[vehicleID] += 1;
            //        ModifyInitialPath(vehicleID, myStep.X, myStep.Y);

            //        _pathfinder.DrawBoard(boardControl1, vehicleID);

            //        //foreach (int subID in sortedVehicleIDs)
            //        //{
            //        //    CalculateVehiclePath(subID);
            //        //}
            //    }
            //}

            int x = -1, y = -1;
            int vehicleID = 1;

            GetVehiclePosition(vehicleID, ref x, ref y);

            if (x > -1 && y > -1)
            {
               lbPositionList.Items.Add("Vehicle #" + vehicleID.ToString() + ": " + x.ToString() + "," + y.ToString());
            }
        }

        private void btnShowPMList_Click(object sender, EventArgs e)
        {
            lbPathList.Items.Clear();
            lbMotionList.Items.Clear();

            int selectedRow = dataGridViewVehicle.CurrentRow.Index;
            int vehicleID = selectedRow + 1;

            CalculateVehiclePath(vehicleID);
        }

        private void btnMoveVehicles_Click(object sender, EventArgs e)
        {
            
            //for (int k = 0; k < dataGridViewVehicle.RowCount - 1; k++)
            //{
            //    int vehicleID;

            //    dataGridViewVehicle.CurrentCell = dataGridViewVehicle.Rows[k].Cells["dgVehicleID"];
            //    vehicleID = Convert.ToInt32(dataGridViewVehicle.CurrentCell.Value);
            //    CalculateVehiclePath(vehicleID);
            //}

            // sort the agents according to their priorities
            
            int rowsCount;
            rowsCount = dataGridViewVehicle.Rows.Count;

            for (int rownum = 0; rownum < rowsCount - 1; rownum++)
            {
                sortedVehicleIDs.Add(rownum + 1);

                int pr;

                dataGridViewVehicle.CurrentCell = dataGridViewVehicle.Rows[rownum].Cells["dgPriority"];
                pr = Convert.ToInt32(dataGridViewVehicle.CurrentCell.Value);
                sortedVehiclePriorities.Add(pr);
            }

            //int i, j;
            //int tmpint;
            //for (i = 1; i < sortedVehiclePriorities.Count; i++)
            //{
            //    j = i;
            //    while (sortedVehiclePriorities[j - 1] < sortedVehiclePriorities[j])
            //    {
            //        tmpint = sortedVehiclePriorities[j - 1];
            //        sortedVehiclePriorities[j - 1] = sortedVehiclePriorities[j];
            //        sortedVehiclePriorities[j] = tmpint;

            //        tmpint = sortedVehicleIDs[j - 1];
            //        sortedVehicleIDs[j - 1] = sortedVehicleIDs[j];
            //        sortedVehicleIDs[j] = tmpint;

            //        j--;
            //        if (j == 0) break;
            //    }
            //}

            timerMoveAgents.Enabled = true;
        }

        private void btnTask_Click(object sender, EventArgs e)
        {
            MessageBox.Show("Not yet :(");
        }
    }
}
